﻿using System;
using System.Text;
using System.Drawing;
using System.Buffers;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;

public static partial class NativeAOT
{
    [UnmanagedCallersOnly(EntryPoint = "grad")]
    public static unsafe void grad(IntPtr a_ptr, int n, IntPtr b_ptr, double eps, IntPtr x_ptr)
    {
        double* a = (double*)a_ptr.ToPointer();
        double* b = (double*)b_ptr.ToPointer();
        double* x = (double*)x_ptr.ToPointer();

        grad(a, n, b, eps, x);
    }

    /// <summary>
    /// 共轭梯度法
    /// </summary>
    /// <param name="a">a[n][n]存放对称正定矩阵。</param>
    /// <param name="n">方程组阶数。</param>
    /// <param name="b">b[n]存放方程组右端常数向量。</param>
    /// <param name="eps"> 控制精度要求。</param>
    /// <param name="x">x[n]返回方程组解向量。</param>
    public static unsafe void grad(double* a, int n, double* b, double eps, double* x)
    {
        int i, k;
        double alpha, beta, d, e;
        double* p = stackalloc double[n];
        double* r = stackalloc double[n];
        double* s = stackalloc double[n];
        double* q = stackalloc double[n];

        for (i = 0; i <= n - 1; i++)
        {
            x[i] = 0.0;
            p[i] = b[i];
            r[i] = b[i];
        }
        i = 0;
        while (i <= n - 1)
        {
            tmul(a, n, n, p, n, 1, s);
            d = 0.0;
            e = 0.0;
            for (k = 0; k <= n - 1; k++)
            {
                d = d + p[k] * b[k];
                e = e + p[k] * s[k];
            }
            alpha = d / e;
            for (k = 0; k <= n - 1; k++)
            {
                x[k] = x[k] + alpha * p[k];
            }
            tmul(a, n, n, x, n, 1, q);
            d = 0.0;
            for (k = 0; k <= n - 1; k++)
            {
                r[k] = b[k] - q[k];
                d = d + r[k] * s[k];
            }
            beta = d / e; d = 0.0;
            for (k = 0; k <= n - 1; k++)
            {
                d = d + r[k] * r[k];
            }
            d = Math.Sqrt(d);
            if (d < eps)
            {
                return;
            }
            for (k = 0; k <= n - 1; k++)
            {
                p[k] = r[k] - beta * p[k];
            }
            i = i + 1;
        }
        return;
    }

    /*
      int main()
      { 
          int i;
          double eps,x[4];
          double a[4][4]={{5.0,7.0,6.0,5.0},
                               {7.0,10.0,8.0,7.0},
                               {6.0,8.0,10.0,9.0},
                               {5.0,7.0,9.0,10.0}};
          double b[4]={23.0,32.0,33.0,31.0};
          eps=0.000001;
          grad(&a[0][0],4,b,eps,x);
          for (i=0; i<=3; i++)  cout <<"x(" <<i <<") = " <<x[i] <<endl;
          return 0;
      }
    */
}
